import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import TimerAction
from launch_ros.actions import Node

def generate_launch_description():
    package_path = get_package_share_directory("description_pkg")
    # 柱子
    pillar_path =os.path.join(package_path, 'models', 'pillar.sdf')
    ld = LaunchDescription()

    # 定义行列数
    cols = 6
    rows = 10

    # 定义每个柱子之间的间距
    spacing_x = 8.0
    spacing_y = 8.0
    # 如果列数是奇数，则将起始位置向右移动一个间距的一半
    if rows % 2 != 0:
        start_x = 0-(rows+1)/2 * spacing_x+spacing_x/2
    else: 
        start_x = 0-rows/2 * spacing_x+spacing_x/2
    if cols % 2 != 0:
        start_y = 0-(cols+1)/2 * spacing_y+spacing_y/2
    else:
        start_y = 0-cols/2 * spacing_y+spacing_y/2
    # 创建柱子节点
    for col in range(cols):
        for row in range(rows):
            spawn_pillar = Node(
                package='gazebo_ros',
                executable='spawn_entity.py',
                name=f'spawn_entity_{col}_{row}',
                arguments=[
                    '-file', pillar_path,
                    '-entity', f'pillar_{col}_{row}',
                    '-x', str(row * spacing_x + start_x),
                    '-y', str(col * spacing_y + start_y),
                    '-z','2.5', # 柱子高度/2
                    '-Y', '0.0'
                ]
            )
            # 打印
            # print(f'Spawning pillar_{col}_{row} at ({row * spacing_x + start_x}, {col * spacing_y + start_y})')
            # 使用 TimerAction 添加延迟
            spawn_delay = 2.0 + (col * rows + row) * 1.0  # 每个实体增加 n 秒的延迟
            ld.add_action(TimerAction(period=spawn_delay, actions=[spawn_pillar]))

    return ld